Skip to content

Commit

Permalink
compensate for lag due to servo period
Browse files Browse the repository at this point in the history
  • Loading branch information
jmkasunich authored and SebKuzminsky committed Jun 21, 2013
1 parent 36d9144 commit 1e2ac52
Showing 1 changed file with 5 additions and 1 deletion.
6 changes: 5 additions & 1 deletion src/hal/components/bldc.comp
Expand Up @@ -385,6 +385,7 @@ variable char old_ph = 000;
variable int old_pattern = -1;
variable double counter = 0;
variable __s64 long_rawcounts = 0;
variable __s64 old_long_rawcounts = 0;
variable __s32 old_rawcounts = 0;
variable int force_trap = 0;

Expand Down Expand Up @@ -468,6 +469,7 @@ FUNCTION(_) {
double V;
double lead;
double sintheta, costheta;
long long lagcomped_counts;
char ph;
int trap = -1; // Flag for sinusoidal/trapezoidal output

Expand All @@ -489,6 +491,7 @@ FUNCTION(_) {

if (personality & 0x06) {
// Handle s32 rollover of rawcounts
old_long_rawcounts = long_rawcounts;
long_rawcounts += (rawcounts-old_rawcounts);
old_rawcounts = rawcounts;
}
Expand Down Expand Up @@ -672,7 +675,8 @@ FUNCTION(_) {
} else {
lead = lead_angle / 360.0;
}
rotor_angle = (double)((long_rawcounts - offset_measured
lagcomped_counts = long_rawcounts + ((long_rawcounts - old_long_rawcounts)/2);
rotor_angle = (double)((lagcomped_counts - offset_measured
- encoder_offset)* poles/2)/scale;
rotor_angle -= floor(rotor_angle);
phase_angle = rotor_angle + lead;
Expand Down

0 comments on commit 1e2ac52

Please sign in to comment.